#include <sstream>
#include <vector>
#include "robot_manager.h"
#include "utilities/convert_data_type.hpp"
#include "utilities/file.hpp"
void RobotManager::commandTeachMoveCallback(const std_msgs::msg::Int32::SharedPtr msg)
{
    CmdMsg msg_;
    msg_.int32 = std::move(msg);
    {
        std::unique_lock<std::mutex> lock(mutex_);
        move_msg_queue_.emplace(CmdType::TEACH_MOVE, msg_);
    }
    cv_.notify_one();
}

void RobotManager::teachMoveFunc(const CmdMsg &msg)
{
    //! 不支持仿真
    if (is_simulation_)
    {
        ZLOGE("simulation not support in this mode!");
        return;
    }
    if (msg.int32->data == 1)
    {
        ZLOGI("enable zero-force drag mode...");
        rclcpp::Rate rate(CONTROL_FREQ);
        Eigen::VectorXd q, qd, qdd;
        Eigen::VectorXd torque;
        std::vector<std::vector<double>> q_sets;
        double time = 0.0;
        while (1)
        {
            pthread_mutex_lock(&zoros_mutex_);
            q = Vector2Eigen(joint_state_msg_.position);
            robot_algo_->id_->inverseDynamics(q, Eigen::VectorXd::Zero(7), Eigen::VectorXd::Zero(7), torque);
            // qd = joint_state_msg_.velocity;
            for (size_t i = 0; i < motors_.size(); i++)
            {
                dm_->control_mit(motors_[i], 0.0, 0.0, 0.0, 0.0, torque[i] * torque_gain_[i]);
            }
            // if(time < 10)
            // {
            //     q_sets.push_back(Eigen2Vector(q));
            // }
            pthread_mutex_unlock(&zoros_mutex_);
            time += 0.004;
            rate.sleep();

            if (time > 10)
            {
                // WriteDataFile("./data_" + GetCurrentTimeString() + ".txt", q_sets, ',');
                // ZLOGI("record pass 10 seconds data!");
                // q_sets.clear();
                time = 0;
            }
        }
    }
}
